// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
//-
// Copyright (c) 2010 Michael Smith. All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
// OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
// SUCH DAMAGE.

/// @file       markup.pde
/// @brief      parser/renderer for display markup microlanguage
///

/// Display strings are marked up with cursor control (newlines)
/// and variable substitution requests, which are bytes with the
/// high bit set.
///
/// Newlines cause the cursor to move down one row and back to
/// column 0.
///
/// Variables are identified by the low seven bits of the byte,
/// and are always printed in their "usual" format.
/// If the low bits are all zero the byte is treated as an escape
/// and the next character is emitted without processing.
///
/// Some assumptions about the programmable character set of the 
/// display are made:
///
/// \x0 reserved for link status character
/// \x1 "roll left"
/// \x2 "roll right"
/// \x3 "arrow up"
/// \x4 "arrow down"
/// \x5 "to the power of minus 1"
/// \x6 --unused-- (battery?)
/// \x7 --unused---
///
/// Useful characters from the LCD character set:
///
/// \x1e right arrow
/// \x1f left arrow
/// \xdf degrees

#define GLYPH_ROLL_LEFT         '\x1'
#define GLYPH_ROLL_RIGHT        '\x2'
#define GLYPH_PITCH_UP          '\x3'
#define GLYPH_PITCH_DOWN        '\x4'
#define GLYPH_YAW_RIGHT         '\x7e'
#define GLYPH_YAW_LEFT          '\x7f'

PROGMEM const prog_char stringModesString[] =
        "MANUAL\0"
        "CIRCLE\0"
        "STABILIZE\0"
        "3\0"
        "4\0"
        "FBW A\0"
        "FBW B\0"
        "7\0"
        "8\0"
        "9\0"
        "AUTO\0"
        "RTL\0"
        "LOITER\0"
        "TAKEOFF\0"
        "LAND\0";
Stringtab       stringModes(stringModesString);

void
Markup::emit(const prog_char *str)
{
        uint8_t         c;
        uint8_t         row;
        bool            escaped;

        // setup
        /// @todo support rendering other than from top left
        lcd.setCursor(0, 0);
        row = 0;
        escaped = false;

        // walk the format string
        for (;;) {
                c = pgm_read_byte_near(str++);
                // end of format string
                if (0 == c)
                        break;
                if (escaped) {
                        lcd.write(c);
                        escaped = false;
                        continue;
                }
                // newline?
                if ('\n' == c) {
                        lcd.setCursor(0, ++row);
                        continue;
                }
                // escape?
                if (0x80 == c) {
                        escaped = true;
                        continue;
                }
                // substitution?
                if (0x80 & c) {
                        _substitute(c & 0x7f);
                        continue;
                }
                // emit
                lcd.write(c);
        }
}

// printf by any other name...
void
Markup::_substitute(uint8_t key)
{
        bool            available;
        uint8_t         fieldWidth;      // number of character positions to use
        uint8_t         decPos;         // buffer position for decimal point
        char            decBuf[12];
        uint8_t         i;
        bool            negative;
        union {
                const prog_char *c;
                uint32_t        u;
                int32_t         s;
        } value;
        enum {
                STRING,         // character string in program memory
                UNSIGNED,       // unsigned value
                SIGNED          // signed value with optional leading -
        } format;

        // test for value availability
        available = _available(key);
        if (!available) {
                // default to a string of dashes - individual handlers will set width
                value.c = PSTR("----------");
                format = STRING;
        }
        fieldWidth = 0;
        decPos = 0;
        negative = false;

        switch (key) {
        case V_FLIGHTMODE:
                fieldWidth = 9;
                format = STRING;
                if (available) {
                        value.c = stringModes.lookup(_heartbeat.flightMode);
                        if (NULL == value.c)
                                value.c = PSTR("??MODE??");
                }
                break;
        case V_TIME:
                fieldWidth = 5;
                if (available) {
                        format = UNSIGNED;
                        value.u = _heartbeat.timeStamp;
                }
                break;
        case V_VOLTAGE:
                fieldWidth = 4;
                if (available) {
                        format = UNSIGNED;
                        decPos = 1;
                        value.u = _heartbeat.batteryVoltage / 100;
                }
                break;
        case V_ROLL:
                if (available) {
                        fieldWidth = 2;
                        format = UNSIGNED;
                        value.u = _subAttitude(_attitude.roll / 100, GLYPH_ROLL_LEFT, GLYPH_ROLL_RIGHT);
                } else {
                        fieldWidth = 3;
                }
                break;
        case V_PITCH:
                if (available) {
                        fieldWidth = 2;
                        format = UNSIGNED;
                        value.u = _subAttitude(_attitude.pitch / 100, GLYPH_PITCH_UP, GLYPH_PITCH_DOWN);
                } else {
                        fieldWidth = 3;
                }
                break;
        case V_YAW:
                if (available) {
                        fieldWidth = 2;
                        format = UNSIGNED;
                        value.u = _subAttitude(_attitude.yaw / 100, GLYPH_YAW_LEFT, GLYPH_YAW_RIGHT);
                } else {
                        fieldWidth = 3;
                }
                break;
        case V_LATITUDE:
                fieldWidth = 8;
                if (available) {
                        format = SIGNED;
                        decPos = 5;
                        value.s = _location.latitude / 100;
                }
                break;
        case V_LONGITUDE:
                fieldWidth = 9;
                if (available) {
                        format = SIGNED;
                        decPos = 5;
                        value.s = _location.longitude / 100;
                }
                break;
        case V_ALTITUDE:
                fieldWidth = 4;
                if (available) {
                        format = SIGNED;
                        value.s = constrain((_location.altitude / 10), -999, 9999);
                }
                break;
        case V_GROUNDSPEED:
                fieldWidth = 3;
                if (available) {
                        format = SIGNED;
                        // 360km/h backwards is probably enough
                        value.s = constrain(_location.groundSpeed / 100, -99, 999);
                }
                break;
        case V_GROUNDCOURSE:
                fieldWidth = 3;
                if (available) {
                        format = UNSIGNED;
                        value.u = constrain(_location.groundCourse / 1000, 0, 359);
                }
                break;
                // XXX ADD MORE HERE
        default:
                Serial.print("unrecognised format code ");Serial.println((int)key, 16);
                // debugging hint
                fieldWidth = 1;
                format = STRING;
                value.c = PSTR("?");
                break;
        }

        switch (format) {
        case STRING:
                while (fieldWidth--) {
                        i = pgm_read_byte_near(value.c);
                        if (0 == i) {
                                lcd.write(' ');
                        } else {
                                lcd.write(i);
                                value.c++;
                        }
                }
                break;
        case SIGNED:
                if (value.s < 0) {
                        negative = true;
                        value.u = -value.s;
                } else {
                        value.u = value.s;      // XXX arguably a NOP
                }
                // FALLTHROUGH
        case UNSIGNED:
                decBuf[0] = '0' + (value.u % 10);
                value.u /= 10;
                for (i = 1; i < fieldWidth; i++) {
                        if (i == decPos) {
                                decBuf[i] = '.';
                        } else if ((0 == value.u) && (i > (decPos + 1))) {
                                decBuf[i] = ' ';
                        } else {
                                decBuf[i] = '0' + (value.u % 10);
                                value.u /= 10;
                        }
                }
                if (negative)
                        decBuf[i++] = '-';
                while (i-- > 0)
                        lcd.write(decBuf[i]);
                break;
        }
}

uint16_t
Markup::_subAttitude(int16_t value, uint8_t glyphNegative, uint8_t glyphPositive)
{
        if (value < 0) {
                lcd.write(glyphNegative);
                return(constrain(-value, 0, 99));
        }
        if (value > 0) {
                lcd.write(glyphPositive);
                return(constrain(value, 0, 99));
        }
        lcd.write(' ');
        return(0);
}

bool
Markup::_available(uint8_t key)
{
        uint8_t word, bit;

        // compute index for availability bit
        word = key / 16;
        bit = key % 16;

        // test and return
        return((_availability[word] & (1U << bit)) ? true : false);
}

void
Markup::message(void *arg, uint8_t messageID, uint8_t messageVersion, void *buf)
{
        ((Markup *)arg)->_message(messageID, messageVersion, buf);
}

void
Markup::_message(uint8_t messageID, uint8_t messageVersion, void *buf)
{
        void    *dst;
        size_t  len;

        switch (messageID) {
        case MSG_HEARTBEAT:
                dst = &_heartbeat;
                len = sizeof(_heartbeat);
                _availability[0] |= AVAIL_HEARTBEAT;
                break;
                
        case MSG_ATTITUDE:
                dst = &_attitude;
                len = sizeof(_attitude);
                _availability[0] |= AVAIL_ATTITUDE;
                break;

        case MSG_LOCATION:
                dst = &_location;
                len = sizeof(_location);
                _availability[0] |= AVAIL_LOCATION;
                break;

        case MSG_PRESSURE:
                dst = &_pressure;
                len = sizeof(_pressure);
                _availability[0] |= AVAIL_PRESSURE;
                break;

        default:
                dst = NULL;
        }
        if (NULL != dst)
                memcpy(dst, buf, len);
}

